using System;

namespace Diversity.Robotics.Navigation
{
    /// <summary>
    /// A Combination of origin, beam model and cone model
    /// </summary>
    public class DistanceSensor
    {
        private double _x;
        private double _y;
        private double _theta;

        private bool _useServo;
        private double _sensorOffsetX;
        private double _sensorOffsetY;
        private double _sensorOffsetTheta;

        private BeamModel _beamModel;
        private IConeModel _coneModel;

        /// <summary>
        /// The fixed X offset relative to the robot in cm
        /// </summary>
        public double X
        {
            get { return _x; }
            set { _x = value; }
        }

        /// <summary>
        /// The fixed Y offset relative to the robot in cm
        /// </summary>
        public double Y
        {
            get { return _y; }
            set { _y = value; }
        }

        /// <summary>
        /// The fixed Angle offset relative to the robot in radians
        /// </summary>
        public double Theta
        {
            get { return _theta; }
            set { _theta = value; }
        }

        /// <summary>
        /// The Beam model used for data interpretation
        /// </summary>
        public BeamModel Beam
        {
            get { return _beamModel; }
            set { _beamModel = value; }
        }

        /// <summary>
        /// The cone model used to draw the map update
        /// </summary>
        public IConeModel Cone
        {
            get { return _coneModel; }
            set { _coneModel = value; }
        }

        /// <summary>
        /// Creates a DistanceSensor object which defines where the readings originate and how to interpret them.
        /// If you want to attach to a servo, specify the coordinate of the servo's center of rotation.
        /// </summary>
        /// <param name="x">The fixed X offset relative to the robot in cm</param>
        /// <param name="y">The fixed Y offset relative to the robot in cm</param>
        /// <param name="theta">The fixed Angle offset relative to the robot in radians</param>
        /// <param name="beamModel">The Beam model used for data interpretation</param>
        /// <param name="coneModel">The cone model used to draw the map update</param>
        public DistanceSensor(double x, double y, double theta, BeamModel beamModel, IConeModel coneModel)
        {
            _x = x;
            _y = y;
            _theta = theta;
            _beamModel = beamModel;
            _coneModel = coneModel;
        }

        /// <summary>
        /// This adds an extra kinematic link into the interpretation of where the
        /// sensor is.
        /// CreateReading(distance, angleOffset)
        /// will now rotate the point _sensorOffsetX, _sensorOffsetY
        /// around _x, _y by angleOffset, then add sensorOffsetTheta.
        /// </summary>
        /// <param name="sensorOffsetX">The x offset relative to the axis of rotation</param>
        /// <param name="sensorOffsetY">The y offset relative to the axis of rotation</param>
        /// <param name="sensorOffsetTheta">The angle relative to the servo's coordinate system</param>
        public void AttachToServo(double sensorOffsetX, double sensorOffsetY, double sensorOffsetTheta)
        {
            _useServo = true;
            _sensorOffsetX = sensorOffsetX;
            _sensorOffsetY = sensorOffsetY;
            _sensorOffsetTheta = sensorOffsetTheta;
        }

        public DistanceReading GetReading(Map map, Pose2D state, double percentNoise)
        {
            double mapScale = map.Scale;
            double realOriginX = state.X + (Math.Cos(state.Theta) * _x) - (Math.Sin(state.Theta) * _y);
            double realOriginY = state.Y + (Math.Sin(state.Theta) * _x) + (Math.Cos(state.Theta) * _y);

           // int readingOriginX = (int)Math.Round(realOriginX / mapScale);
           // int readingOriginY = (int)Math.Round(realOriginY / mapScale);
            
            //raycast from here
            double cos = Math.Cos(_theta + state.Theta);
            double sin = Math.Sin(_theta + state.Theta);
            int steps = (int)Math.Round(_beamModel.MaxRange / mapScale);
            for (int i = 1; i <= steps; i++)
            {
                int x = (int)Math.Round((realOriginX + (i * map.Scale * cos)) / mapScale);  //(int)Math.Round(readingOriginX + (i * cos));
                int y = (int)Math.Round((realOriginY + (i * map.Scale * sin)) / mapScale);
                if (map.IsOccupied(x, y))
                {
                   // calculate the true range
                    double realHitX = (x + 0.5) * mapScale;
                    double realHitY = (y + 0.5) * mapScale;
                    double realRange = Math.Min(Math.Sqrt(Math.Pow(realOriginX - realHitX, 2) + Math.Pow(realOriginY - realHitY, 2)),_beamModel.MaxRange);
                    //Console.Out.WriteLine("Converted " + x + "," + y + "," + i + " to " + realHitX + "," + realHitY + "," + realRange);
                    //return new DistanceReading(_x, _y, _theta, realRange, _beamModel, _coneModel);
                    return CreateReading(realRange);
                }
            }
            return CreateReading(_beamModel.MaxRange);
        }

        /// <summary>
        /// Creates a Distance Reading at the distance sensor's origin
        /// </summary>
        /// <param name="distance"></param>
        /// <returns></returns>
        public DistanceReading CreateReading(double distance)
        {
            return  new DistanceReading(_x,_y,_theta,distance,_beamModel,_coneModel);
        }

        /// <summary>
        /// Creates a Distance Reading relative to the robot rotated by angleOffeset degrees.
        /// If this DistanceSensor has been attached to a servo, the servo is rotated.
        /// </summary>
        /// <param name="distance"></param>
        /// <param name="angleOffset"></param>
        /// <returns></returns>
        public DistanceReading CreateReading(double distance, double angleOffset)
        {
            if (!_useServo)
            {
                return new DistanceReading(_x, _y, _theta + angleOffset, distance, _beamModel, _coneModel);
            } 
            else
            {
                double cosTheta = Math.Cos(angleOffset);
                double sinTheta = Math.Sin(angleOffset);
                double dx = cosTheta * _sensorOffsetX - sinTheta * _sensorOffsetY;
                double dy = sinTheta * _sensorOffsetX + cosTheta * _sensorOffsetY;

                return new DistanceReading(_x + dx, _y + dy, _theta + angleOffset + _sensorOffsetTheta, distance, _beamModel, _coneModel);
            }
        }

         

    }
}
